00100		TITLE SERVO
00200		.INSERT ASMBL.FAI[CMS,LCS]
00300	
00400	;I/O address definitions.
00500	   DAC ← 100000	;8 bit DAC.
00600	   JCR ← 120000	;Joint control output register.
00700	   ENCL ← 140000	;Encoder mux low.
00800	   ENCH ← 140001	;Encoder mux high.
00900	
01000	   STKSIZ ← 377	;Stack size.
01100	   LSBENB ← 40	;Enable LSB servo.
01200	
01300	;Zero page variables.
01400	;Not shared.
01500	
01600	DSPAT:	BLOCK 2	;Dispatch address for commands.
01700	DEFCMD:	0	;Deferred command.
01900	SAVPOS:	BLOCK 2	;Position for deferred servo command.
02000	
02100	CMDVEL:	BLOCK 2	;Commanded velocity.
02200	CURVEL:	BLOCK 2	;Current velocity.
02300		0	;SETPT-1.
02400	SETPT:	BLOCK 2	;Current setpoint.
02500		0	;SETINC-1.
02600	SETINC:	BLOCK 2	;Interpolating increment for setpoints.
02700		0	;INTINC-1.
02800	INTINC:	BLOCK 2	;Interpolating increment for velocity.
03000	OLDSP:	BLOCK 2	;Last commanded setpoint, for CMDVEL.
03100	POSERR:	BLOCK 2	;Current position error.
03200	DACSIG:	BLOCK 2	;Scratch.
03300	
03400	INCTR:	0	;Count the interpolations.
03500	HSTTMR:	0	;Count ticks between host commands.
03600	
03700	LOGTMP:	BLOCK 4	;Temp for the arithmetic routines.
03800	
03900		0	;IVEL-1.
04000	IVEL:	BLOCK 2	;Interpolated velocity.
04300	VPTR:	0	;Velocity averaging index.
04400	VELTBL:	BLOCK 20;Velocity averaging table.
04500	
04600	ZAPEND ← .-1	;Clear all the above in startup.
04700	
04800	TL:	0	;Scratch for gray to binary.
04900	TH:	0
05000	
05100	MTMP:	BLOCK 2	;Copy of multipilcand from shared memory.
     

00100	;Shared ram.
00200	   LOC 200	;Second half of zero page.
00300	
00400	;STATUS byte bits.
00500	;	7	6	5	4	3	2	1	0
00600	;    error   check    time     no      bad
00700	;     flag    word     out    tick     pos
00800	
00900		0	;Locked.
01000	STATUS:	0	;Flags for the host.
01100	   ;Fix STATUS and MODE words for host lockout?
01200	;MODE byte bits.
01300	;Bit	7	6	5	4	3	2	1	0
01400	;    servo   integ     lsb    diag
01500	;     enlb    enlb    enbl    enbl
01600	
01700		0	;Locked.
01800	MODE:	0	;Mode bits from host.
01900	
02000	CKWORD:	BLOCK 2	;Host I/O check/command word.
02100	CMDPOS:	BLOCK 2	;Commanded position from host.
02200	
02300	;IOCTRL byte bits.
02400	;Bit	7	6	5	4	3	2	1	0
02500	;      in		      lsb   integ     pos
02600	;     tol		     enlb  disabl    mode
02700	
02800		0	;Locked.
02900	IOCTRL:	0	;Copy of JCR output port.
03000	
03100	CURPOS:	BLOCK 2	;Current position.
03200	
03300		0	;Locked.
03400	NINTER:	0	;# of interpolations between position
03500			;commands.
03600		0	;Locked.
03700	INTSCL:	0	;# of bits to shift setpoint dif for
03800			;interpolating.
03900		0	;Locked.
04000	HSTLIM:	0	;# of clock ticks allowed between host
04100			;commands.
04200	FRICTN:	BLOCK 2	;Viscous damping coefficient.
04300	GRAVTY:	BLOCK 2	;DC offset for gravity.
04400	POSTOL:	BLOCK 2	;Half-width of position tolerance band.
04500	INTTOL:	BLOCK 2	;Half-width of integration band.
04600	PGAIN:	BLOCK 2	;Position error gain.
04700	
04720	VELERR:	BLOCK 2	;Velocity error.
04740	VSUM:	BLOCK 2	;Sum of last eight velocitys.
04800	
04900	;Start of prom.
05000	   LOC 174000
05100	
05200	INITBL:	STATUS	↔	0
05300	
05400		NINTER	↔	=16	;Number of interpolations default.
05500		INTSCL	↔	4	;Interpolating scale default.
05600	
05700		HSTLIM	↔	=24	;Ticks before host time out default.
05800	
05900		377	;End of INITBL flag.
     

00100	;Power on reset.
00200	START:	CLD
00300		LDXI	STKSIZ	;Setup stack.
00400		TXS
00500	
00600		LDAI	0
00700		LDXI	ZAPEND
00800	RLOOP:	STAZX	0	;Reset ram.
00900		DEX
01000		BPL	RLOOP
01100		STA	DAC	;Clear DAC.
01200		LDXI	370	;-8.
01300	ZSR:	STAZX	FRICTN+10	;Clear shared ram.
01400		INX
01500		BMI	ZSR
01600	
01700		LDXI	13	;13,,100 is 40013 is 1.0.
01800		LDYI	100
01900		STXZ	PGAIN	;Reset PGAIN to 1.
02000		STYZ	PGAIN+1	;Unlock.
02100		LDXI	13	;13,,275 is 136413 is -0.125.
02200		LDYI	275
02300		STXZ	FRICTN	;Reset velocity gain to 1.
02400		STYZ	FRICTN+1;Unlock.
02500	
02600		TAY	;Y ← 0.
02700		BEQ	RSTDEF	;Jump
02800	DLOOP:	INY
02900		LDAY	INITBL	;Init ram.
03000		STAZX	0
03100		INY
03200	RSTDEF:	LDXY	INITBL
03300		CPXI	377
03400		BNE	DLOOP
03500	
03600	STOP:	SEI	;Go into stop mode.
03700		JSR	GETPOS	;Read encoder and convert to binary.
03800	;Sets the current position to the converted encoder value, the
03900	;setpoint the same, clears the setpoint interpolating increment,
04000	;and goes into stop mode.
04100		STAZ	CURPOS	;Set the current position.
04200		STXZ	CURPOS+1;Unlock.
04300		STAZ	SETPT	;Set the setpoint.
04400		STXZ	SETPT+1
04500		STAZ	OLDSP	;For CMDVEL.
04600		STXZ	OLDSP+1
04700		LDAI	71	;Reset I/O control bits. Position mode off.
04800		STAZ	IOCTRL
04900		STA	JCR
05000	
05100		LDAI	0
05200		STAZ	MODE	;Clear position servo enable, etc..
05300		STAZ	DEFCMD	;Clear the deferred command flag.
05400		STAZ	SETPT-1	;Clear the setpoint extension
05500		STAZ	SETINC-1;and the interpolator.
05600		STAZ	SETINC
05700		STAZ	SETINC+1
     

00100		LDXI	23
00200	CLRVEL:	STAZX	IVEL-1	;Clear velocity values.
00300		DEX
00400		BPL	CLRVEL
00420	
00440		STAZ	VELERR	;Clear velocity error and sum.
00460		STAZ	VELERR+1;Unlock.
00480		STAZ	VSUM	;Lock.
00490		STAZ	VSUM+1	;Unlock.
00500		CLI	;End of reset.
00600	
00700	RSTCKW:	LDAI	377	;Reset check word.
00800		LDXI	0
00900		SEI
01000		STAZ	CKWORD	;Lock.
01100		STXZ	CKWORD+1;Unlock.
01200		CLI
01300	;Idle loop. Wait for command.
01400	IDLE:	LDAZ	CKWORD+1;Check for new check word.
01500		BEQ	IDLE	;Not equal if bit 7 is complement of low byte.
01600	
01700		SEC
01800		SEI
01900		ADCZ	CKWORD	;Lock.
02000		LDXZ	CKWORD+1;Unlock.
02100		CLI
02200		TAY
02300		BNE	CKWDER	;Check word error.
02400	   ;Check here for immediate or deferred.
02500		TXA	;Check for valid command.
02600		ORAI	3	;3 for two commands and bit 0 = 0.
02700		ADCI	0	;Carry = 1.
02800		BNE	CKWDER	;Not a valid command.
02900	
02920	   ;Valid host command.
02940		LDAZ	HSTLIM	;Reset host timer.
02960		STAZ	HSTTMR
03000		LDAZ	DEFCMD	;Check if no TICK?
03100		BNE	NOTICK	;No response since last deferred command.
03200	
03250	   ;Check here if posiition command?
03300		SEI
03400		LDYZ	CMDPOS	;Read position for servo command.
03500		LDAZ	CMDPOS+1;Unlock.
03600		CLI
03700	
03800		STYZ	SAVPOS	;Save it for later.
03900		STAZ	SAVPOS+1
04000		ASLA	;Check for valid position.
04100		BCS	CSET
04200		BMI	BADPOS
04300		BPL	GOODP	;Jump.
04400	CSET:	BPL	BADPOS
04500	
04600	GOODP:	STXZ	DEFCMD	;Save deferred command pointer.
04700	
04800		JMP	RSTCKW	;Handshake with host via CKWORD.
04900	
05000	CKWDER:	LDAI	300	;Set check word error flag.
05100	WSTAT:	ORAZ	STATUS
05200		STAZ	STATUS
05300		JMP	STOP
05400	
05500	NOTICK:	LDAI	220	;Set tick error flag.
05600		BNE	WSTAT	;Jump.
05700	
05800	BADPOS:	LDAI	210	;Set bad position error flag.
05900		BNE	WSTAT	;Jump.
     

00100	;Clock tick interrupt.
00200	TICK:	PHA	;Save state.
00300		TXA
00400		PHA
00500		TYA
00600		PHA
00700	
00800		INCZ	IOCTRL	;Turn on interrupt flag bit.
00900		LDAZ	IOCTRL	;This is only for timing checks.
01000		STA	JCR	;Can be flushed.
01100	
01200		JSR	GETPOS	;Read position and convert to binary.
01300		SEC
01400		SBCZ	CURPOS	;Subtract the old position
01500		STAZ	CURVEL	;yielding the velocity.
01600		TXA	;High byte of binary position.
01700		SBCZ	CURPOS+1;Unlock.
01800		STAZ	CURVEL+1
01900	
02000		STYZ	CURPOS	;Update the current position.
02100		STXZ	CURPOS+1;Unlock.
02200		DECZ	HSTTMR	;Count the ticks since the last command
02300		BPL	HOSTOK	;and check for timeout.
02400	
02500		LDAI	0	;Host dead. Stop.
02600		STAZ	HSTTMR
02900		STAZ	INTINC-1
03000		STAZ	INTINC
03100		STAZ	INTINC+1
03200		LDAI	240	;Set host time out flag
03300		ORAZ	STATUS
03400		STAZ	STATUS
03500	
03600		LDAI	20	;Check for diagnostic enable.
03700		BITZ	MODE
03800		BNE	HOSTOK	;If diagnostics, then servo anyway.
03900	
04000		LDAI	177	;Turn off servo enable.
04100		ANDZ	MODE
04200		STAZ	MODE
04300	
04400	HOSTOK:	BITZ	MODE	;Check if servo is enabled.
04500		BMI	INTVEL
04900		JMP	CURSRV	;don't servo.
05000	
05100	INTVEL:	CLC	;Interpolate the velocity.
05200		LDAZ	IVEL-1
05300		ADCZ	INTINC-1;IVEL ← IVEL + INTINC.
05400		STAZ	IVEL-1
05500		LDAZ	IVEL
05600		ADCZ	INTINC
05700		STAZ	IVEL
05800		LDAZ	IVEL+1
05900		ADCZ	INTINC+1
06000		STAZ	IVEL+1
     

00100	;Interpolate the setpoints.
00200	INTRS:	CLC
00300		LDAZ	SETPT-1
00400		ADCZ	SETINC-1;Add the increment to the setpoint.
00500		STAZ	SETPT-1
00600		LDAZ	SETPT
00700		ADCZ	SETINC
00800		STAZ	SETPT
00900		LDAZ	SETPT+1
01000		ADCZ	SETINC+1
01100		STAZ	SETPT+1
01200	
01300		DECZ	INCTR	;Check if this is the last interpolation.
01400		BNE	GPOSER
01500	
01600		LDAI	0	;Clear SETINC if done interpolating.
01700		STAZ	SETINC-1
01800		STAZ	SETINC
01900		STAZ	SETINC+1
02000		STAZ	INTINC-1;Clear INTINC (commanded velocity).
02100		STAZ	INTINC
02200		STAZ	INTINC+1
02300	
02400	;Calculate the position error.
02500	GPOSER:	SEC
02600		LDAZ	CURPOS	;POSERR ← CURPOS - SETPT.
02700		SBCZ	SETPT
02800		STAZ	POSERR
02900		LDAZ	CURPOS+1;Unlock.
03000		SBCZ	SETPT+1
03100		STAZ	POSERR+1
     

00100		BITZ	MODE	;If servo is disabled, we're
00200		BPL	OOTOL	;automatically out of tolerance
00300	
00400		LDAZ	POSERR+1;Test the sign of pos error.
00500		BMI	NEGPER
00600	
00700		LDAZ	POSTOL	;Positive. Compare with tol.
00800		CMPZ	POSERR
00900		LDAZ	POSTOL+1;Unlock.
01000		SBCZ	POSERR+1
01100		BCS	TOLOK	;In tolerance.
01200		BCC	OOTOL	;Jump.
01300	
01400	NEGPER:	CLC	;Negative. Add the tolerance.
01500		LDAZ	POSTOL	;Lock.
01600		ADCZ	POSERR
01700		LDAZ	POSTOL+1;Unlock.
01800		ADCZ	POSERR+1
01900		BCS	TOLOK	;In tolerance.
02000	
02100	OOTOL:	LDAZ	IOCTRL	;Out of tolerance.
02200		ANDI	177	;Turn off the in tolerance
02300		BNE	WCNTRL	;indicator. Jump.
02400	
02500	TOLOK:	LDAZ	IOCTRL	;In tolerance. Turn it on.
02600		ORAI	200
02700	WCNTRL:	STAZ	IOCTRL
02800		STA	JCR	;Copy it to output.
02900	
03000		BITZ	MODE	;If intergration is disabled,
03100		BVC	OOBAND	;turn it off.
03200		LDAZ	POSERR+1;Test sign of position error.
03300		BMI	ADTOL
03400	
03500		LDAZ	INTTOL	;Positive. Compare with tol.
03600		CMPZ	POSERR
03700		LDAZ	INTTOL+1;Unlock.
03800		SBCZ	POSERR+1
03900		BCS	INBAND	;In band. Turn on integrator.
04000		BCC	OOBAND	;Jump.
04100	
04200	ADTOL:	CLC	;Negative. Add the tolerance.
04300		LDAZ	INTTOL	;Lock.
04400		ADCZ	POSERR
04500		LDAZ	INTTOL+1;Unlock.
04600		ADCZ	POSERR+1
04700		BCS	INBAND	;Check if in band.
04800	
04900	OOBAND:	LDAZ	IOCTRL	;Out of band. Turn off
05000		ORAI	10	;integration by setting the
05100		ANDI	357	;control bit. LSB servo off.
05200		BNE	WCTRL2	;Jump.
     

00100	INBAND:	LDAI	LSBENB	;In band. Is LSB servo enabled?
00200		BITZ	MODE
00300		BEQ	RCNTRL
00400		LDAZ	POSERR	;Yes. Is the error exactly 0?
00500		ORAZ	POSERR+1
00600		BNE	RCNTRL
00700	
00800		LDAZ	IOCTRL	;It is. Integration off, LSB
00900		ORAI	30	;servo on.
01000		BNE	WCTRL2	;Jump.
01100	
01200	RCNTRL:	LDAZ	IOCTRL	;LSB disabled or error not zero.
01300		ANDI	347	;LSB servo off, integration on.
01400	WCTRL2:	STAZ	IOCTRL
01500		STA	JCR	;Output it.
01600	
01700		LDXZ	PGAIN	;Copy position gain for multiply.
01800		LDYZ	PGAIN+1	;Unlock.
01900		STXZ	MTMP
02000		STYZ	MTMP+1
02100		LDYZ	POSERR
02200		LDAZ	POSERR+1
02300		JSR	LOG	;Float the position error.
02400		LDXI	MTMP	;Point X to copy of PGAIN.
02500		JSR	MULTIP	;POSERR ← POSERR * PGAIN.
02600		JSR	EXP	;Fix it.
02700		STYZ	POSERR
02800		STAZ	POSERR+1
02900	
03000	;Get the velocity error.
03100		CLC
03200		LDAZ	VSUM	;Lock.
03300		ADCZ	CURVEL	;VSUM ← VSUM + CURVEL.
03400		TAX
03500		LDAZ	VSUM+1	;Unlock.
03600		ADCZ	CURVEL+1
03700		TAY
03800		TXA
03900		LDXZ	VPTR	;Get velocity averaging index.
04000		SEC
04100		SBCZX	VELTBL	;VSUM ← VSUM - VELTBL[VPTR].
04200		STAZ	VSUM	;Lock.
04300		TYA
04400		SBCZX	VELTBL+10
04500		STAZ	VSUM+1	;Unlock.
04600	
04700		LDAZ	CURVEL	;VELTBL[VPTR] ← CURVEL.
04800		STAZX	VELTBL
04900		LDAZ	CURVEL+1
05000		STAZX	VELTBL+10
05100		INX	;VPTR ← (VPTR + 1) .AND. (VTLEN - 1).
05200		TXA
05300		ANDI	7
05400		STAZ	VPTR
05500	
05600		SEC
05700		LDAZ	VSUM	;Lock.
05750		LDXZ	VSUM+1	;Unlock.
05800		SBCZ	IVEL	;VELERR ← VSUM - IVEL.
05850		TAY	;Save VELERR.
05900		STAZ	VELERR	;Lock.
06000		TXA	;Get VSUM+1.
06100		SBCZ	IVEL+1
06200		STAZ	VELERR+1;Unlock.
     

00100		JSR	LOG	;Float the velocity error.
00200		STYZ	MTMP	;Save the F.P. VELERR.
00300		STAZ	MTMP+1
00600		LDYZ	FRICTN	;Get the velocity gain (FRICTN).
00700		LDAZ	FRICTN+1;Unlock.
00800		LDXI	MTMP	;multiply by the velocity error,
00900		JSR	MULTIP	;VELERR * VGAIN.
01000		JSR	EXP
01100	
01200		TAX	;Save high byte.
01300		TYA	;Get low byte.
01400		CLC	;add the position error...
01500		ADCZ	POSERR
01600		TAY	;Save low byte.
01700		TXA
01800		ADCZ	POSERR+1
01900		STAZ	DACSIG+1
02000	
02100		CLC	;...and the gravity offset.
02200		TYA	;Get the low byte.
02300		ADCZ	GRAVTY	;Lock.
02400		TAY	;Save low byte.
02500		LDAZ	GRAVTY+1;Unlock.
02600		ADCZ	DACSIG+1
02700	
02800		JSR	PUTDAC	;Put result out to the DAC.
02900	
03000	CMDSP:	LDAZ	DEFCMD	;Check for a command.
03100		BEQ	INTXIT
03200		ANDI	2	;Low nibble command bit.
03300		TAX
03400		LDAX	CMDTBL	;Get command address.
03500		STAZ	DSPAT
03600		LDAX	CMDTBL+1
03700		STAZ	DSPAT+1
03800		JMPIN	DSPAT	;Execute command.
03900	
04000	CMDEND:	LDAI	0	;Done with deferred command.
04100		STAZ	DEFCMD	;Reset command word.
04200	INTXIT:	DECZ	IOCTRL	;Turn off interrupt flag.
04300		LDAZ	IOCTRL	;Can be flushed.
04400		STA	JCR
04500		PLA	;Restore state and dismiss interrupt.
04600		TAY
04700		PLA
04800		TAX
04900		PLA
05000		RTI
05100	
05200	CMDTBL:		;DEFERRED COMMAND TABLE.
05300		CMDEND∧377	;Nop.
05400		(CMDEND⊗-10)∧377
05500		CMDSRV∧377	;Servo command.
05600		(CMDSRV⊗-10)∧377
     

00100	;Deferred commands.
00200	CMDSRV:	LDAZ	MODE	;Servo command.
00300		BMI	ENBLD	;Test for servo enabled.
00400		JMP	CMDEND	;No. End this command. 
00500	
00600	ENBLD:	LDAZ	SAVPOS	;Enabled. Get commanded position.
00700		SEC
00800		SBCZ	SETPT	;Get difference between next position
00900		STAZ	SETINC	;and the last setpoint.
01000		LDAZ	SAVPOS+1
01100		SBCZ	SETPT+1
01200		LDXI	0
01300		STXZ	SETPT-1	;Clear setpoint and increment extentions.
01400		STXZ	SETINC-1
01500		STXZ	INTINC-1
01600		LDXZ	INTSCL
01700	   ;A = SETINC+1.
01800	SCAL:	CMPI	200	;Extend sign.
01900		RORA	;Divide the difference by the number of interpolations.
02000		RORZ	SETINC
02100		RORZ	SETINC-1
02200		DEX
02300		BNE	SCAL
02400		STAZ	SETINC+1;Which yields the interpolating increment.
02500	
02600		LDAZ	NINTER
02700		STAZ	INCTR	;Setup the interpolator count.
02800		SEC	;INTINC ← ((CMDVEL / 2) - IVEL) / 16.
02900		LDAZ	SAVPOS
03000		SBCZ	OLDSP	;CMDVEL ← CMDPOS - OLDSP.
03100		STAZ	CMDVEL
03200		LDAZ	SAVPOS+1
03300		SBCZ	OLDSP+1
03350		CMPI	200	;Extend sign and divide by 2.
03375		RORA
03387		RORZ	CMDVEL
03393		RORZ	INTINC-1;INTINC = LSB of CMDVEL.
03400		STAZ	CMDVEL+1
03500		LDAZ	SAVPOS
03600		STAZ	OLDSP	;OLDSP ← CMDPOS.
03700		LDAZ	SAVPOS+1
03800		STAZ	OLDSP+1
03900	
04100		SEC
04200		LDAZ	INTINC-1;INTINC-1 = CMDVEL-1.
04300		SBCZ	IVEL-1	;INTINC ← CMDVEL - IVEL.
04400		STAZ	INTINC-1
04420		LDAZ	CMDVEL
04440		SBCZ	IVEL
04460		STAZ	INTINC
04500		LDAZ	CMDVEL+1
04600		SBCZ	IVEL+1
04650		LDXZ	INTSCL	;A = INTINC+1.
04700	ISCAL:	CMPI	200	;Extend sign and divide by the number of
04800		RORA		;interpolations.
04900		RORZ	INTINC
05000		RORZ	INTINC-1
05050		DEX
05075		BNE	ISCAL
05100		STAZ	INTINC+1
     

00100		LDAI	44
00200		ORAZ	IOCTRL	;Turn on servo and current mode enable bits.
00300		STAZ	IOCTRL
00400		STA	JCR	;Output it.
00700		JMP	CMDEND
00800	
00900	;Free mode.
01000	CURSRV:	LDAI	0	;Not servoing ("Current mode")...
01100		STA	DAC	;Turn off the servo valve.
01200		STAZ	SETPT-1	;Make the setpoint track
01300		LDAZ	CURPOS	;the current position in order to
01400		STAZ	SETPT	;keep the arm from twitching when
01500		LDAZ	CURPOS+1;the host enables the servo. Unlock.
01600		STAZ	SETPT+1
01700		LDAI	373
01800		ANDZ	IOCTRL	;Turn off position mode bit.
01900		STAZ	IOCTRL
02000		STA	JCR
02100		JMP	CMDSP	;Go check on commands.
02200	
02300	;DAC output subroutine.
02400	;Enter with 2 byte value in Y (low), A (high).
02500	;Clobbers all registers, but the 8 bits the DAC got are returned in A.
02600	PUTDAC:	BMI	NEGDAC	;Assuming the last inst. loaded A.
02700		CPYI	200	;Positive. Compare with 2↑7.
02800		SBCI	0
02900		BCC	INRNGE
03000	
03100	TOOHI:	LDYI	177	;Too high. Saturate positive.
03200		BNE	INRNGE	;Jump.
03300	
03400	NEGDAC:	CPYI	200	;Negative. Compare with -2↑7.
03500		SBCI	377
03600		BCS	INRNGE
03700	
03800	TOOLOW:	LDYI	200	;Too low. Saturate to -2↑7.
03900	
04000	INRNGE:	LDAY	VETBL	;Straighting it.
04100		STA	DAC	;Output 8 bits to the DAC.
04200		RTS
     

00100	;Position conversion routine.
00200	GETPOS:	LDY	ENCL	;Read encoder.
00300		LDA	ENCH
00400	;Convert from gray to binary.
00500		STAZ	TH
00600		LSRA	;Shift by 1.
00700		EORZ	TH
00800		STAZ	TH
00900		TAX	;X ← high byte.
01000	
01100		TYA
01200		STAZ	TL
01300		RORA
01400		EORZ	TL
01500		STAZ	TL
01600	
01700		LSRZ	TH	;Shift by 2.
01800		RORA
01900		LSRZ	TH
02000		RORA
02100		EORZ	TL
02200		STAZ	TL
02300		TAY	;Y ← low byte.
02400	
02500		TXA	;Get high byte.
02600		EORZ	TH
02700		STAZ	TH
02800	
02900		LSRA	;Shift by 4.
03000		RORZ	TL
03100		LSRA
03200		RORZ	TL
03300		LSRA
03400		RORZ	TL
03500		LSRA
03600		RORZ	TL
03700	
03800		EORZ	TH
03900		STAZ	TH
04000		TYA
04100		EORZ	TL
04200		EORZ	TH	;Shift by 8.
04300		TAY	;Save low byte.
04400	
04500		LDXZ	TH	;Get high byte.
04600		BITZ	TH
04700		BVC	POS	;Check if negative.
04800		TXA
04900		ORAI	200	;Extend sign.
05000		TAX
05100	
05200	POS:	TYA	;Returns with position in A, Y (low) and X (high).
05300		RTS
     

00100	;Arithmetic routines.
00200	;Enter with high byte in A, low in Y.
00300	;Returns A = characteristic and sign, Y = mantissa.
00400	;Clobbers X, LOGTMP, LOGTMP+1.
00500	LOG:	STYZ	LOGTMP	;Save the inputs.
00600		STAZ	LOGTMP+1
00700	
00800		LDXI	20+100	;Init characteristic to 15.
00900		CMPI	0	;Test sign of input.
01000		BPL	POSIN
01100		SEC	;Negative. 2's complement it.
01200		LDAI	0
01300		SBCZ	LOGTMP
01400		STAZ	LOGTMP
01500		LDAI	0
01600		SBCZ	LOGTMP+1
01700	POSIN:	BNE	NORML	;Is high byte zero?
01800		LDAZ	LOGTMP	;Yes. Low byte?
01900		BEQ	RTRN	;If so, return zero.
02000		LDYI	0	;Low nonzero. Shift left one
02100		STYZ	LOGTMP	;byte,
02200		LDXI	10+100	;change characteristic to 7.
02300	NORML:	DEX	;Normalize the number, counting the
02400		ASLZ	LOGTMP	;characteristic down. When the
02500		ROLA	;first "1" shifts out, we've subtracted
02600		BCC	NORML	;1 from the normalized number
02700		ASLZ	LOGTMP	;(This rounds the result)
02800		ADCI	=11	;and are left with the fraction
02900		TAY	;Adding 11 to that is equivalent to
03000		TXA	;adding 0.043.
03100		ADCI	0	;Propagate the carry into the
03200				;characteristic.
03300		ASLA	;Insert the sign bit from the saved
03400		ASLZ	LOGTMP+1;input.
03500		RORA
03600	RTRN:	RTS	;Done.
03700	
03800	;Enter with sign and characteristic in A, mantissa in Y
03900	;Returns 16-bit integer, low byte in Y, high in A.
04000	;Clobbers X, LOGTMP, LOGTMP+1.
04100	EXP:	STAZ	LOGTMP+1;Save sign of input.
04200		ANDI	177	;Mask it off.
04300		BEQ	ZEROIN	;Zero characteristic returns
04400		TAX	;zero.
04500		TYA	;Get the mantissa...
04600		SEC
04700		SBCI	=11	;...subtract 0.043...
04800		STAZ	LOGTMP	;(save this value)
04900		TXA	;...propagate the carry and get rid
05000		SBCI	100	;of the XS-64 offset.
05100		BMI	NEGIN	;If negative (value < 1.0)
05200				;return zero.
05300		CMPI	=15	;Test for overflow (value>=2↑15
05400		BCS	SATUR
05500		TAX	;...no. Number is in range.
05600		ADCI	370	;Is characteristic below 8?
05700		BMI	BLOATE
05800		TAX	;No. Reduce if by 8,
05900		JSR	UNNORM	;unnormalize.
06000		BMI	GETTMP	;Jump.
     

00100	BLOATE:	JSR	UNNORM	;Yes. Unnormalize, then
00200		ASLZ	LOGTMP	;(round result)
00300		ADCI	0
00400		STAZ	LOGTMP	;use result as low byte and
00500		LDAI	0	;set high byte to zero.
00600	
00700	GETTMP:	LDYZ	LOGTMP
00800	GTMP1:	LDXZ	LOGTMP+1;Test sign of input...
00900		BPL	POSIGN
01000		STAZ	LOGTMP+1;...negative. 2's complement
01100		LDAI	0	;the result.
01200		SEC
01300		SBCZ	LOGTMP
01400		TAY
01500		LDAI	0
01600		SBCZ	LOGTMP+1
01700	POSIGN:	RTS
01800	
01900	NEGIN:	LDAI	0	;Set the result to zero if the
02000	ZEROIN:	TAY	;input is negative.
02100		RTS
02200	
02300	SATUR:	LDYI	377	;Saturate result to 2↑15 - 1 if
02400		STYZ	LOGTMP	;input was 15 or more.
02500		LDAI	177
02600		BNE	GTMP1	;Jump.
02700	
02800	UNNORM:	LDAI	1	;Unnormalize subroutine. Add 1
02900		BNE	DECRX	;to the fraction. Jump.
03000	
03100	SCALE:	ASLZ	LOGTMP	;Scale the fraction left by the
03200		ROLA	;amount of the characteristic.
03300	DECRX:	DEX
03400		BPL	SCALE
03500		RTS
     

00100	;Enter with characteristic of multiplier in A,
00200	;mantissa in Y, X pointing to a pair of base page
00300	;locations containing the multiplicand (mantissa in the
00400	;low byte).
00500	;Returns the product in A and Y, same form as the
00600	;multiplier. Leaves X unchanged. Clobbers LOGTMP and
00700	;LOGTMP+1.
00800	MULTIP:	PHA
00900		EORZX	1	;Compute sign of result,
01000		STAZ	LOGTMP+1	;save it away.
01100		PLA
01200		ANDI	177	;Mask off multiplier sign.
01300		BEQ	ZEROIN	;If zero, return zero.
01400		STAZ	LOGTMP
01500		TYA	;Add the two logarithms.
01600		CLC
01700		ADCZX	0
01800		TAY
01900		LDAZX	1
02000		ANDI	177	;If multiplicand is zero,
02100		BEQ	ZEROIN	;return a zero.
02200		ADCZ	LOGTMP
02300		SEC
02400		SBCI	100	;Correct the XS-64 offset.
02500		BPL	INSIGN	;Result in range?
02600		ANDI	100	;No. If underflow,
02700		BNE	NEGIN	;return zero.
02800		LDAI	177	;Overflow. Saturate to
02900		LDYI	377	;highest magnitude.
03000	
03100	INSIGN:	ASLA	;Insert the sign of the result.
03200		ASLZ	LOGTMP+1
03300		RORA
03400		RTS
03500	
03600	;Inverse function: 2's complement the magnitude part
03700	;of a 15-bit logarithm.
03800	;Enter with characteristic in A, mantissa in Y.
03900	;Returns inverse in the same form. X unchanged.
04000	;Clobbers LOGTMP and LOGTMP+1.
04100	INV:	STYZ	LOGTMP	;Pretty straightforward...
04200		STAZ	LOGTMP+1
04300		SEC
04400		LDAI	0	;Complement the number by
04500		SBCZ	LOGTMP	;subtracting it from zero.
04600		TAY
04700		LDAI	0
04800		SBCZ	LOGTMP+1
04900		JMP	INSIGN	;Insert the original sign.
     

00100	;DAC output table.
00200	   LOC (.∨377)+1	;For start of next page.
00300	VETBL:		;DAC output table.
00400	  0 ↔  20 ↔  26 ↔  33 ↔  37 ↔  43 ↔  46 ↔  50
00500	 52 ↔  54 ↔  56 ↔  57 ↔  60 ↔  62 ↔  63 ↔  64
00600	 65 ↔  66 ↔  67 ↔  70 ↔  71 ↔  72 ↔  73 ↔  74
00700	 75 ↔  76 ↔  76 ↔  77 ↔ 100 ↔ 101 ↔ 102 ↔ 103
00800	104 ↔ 104 ↔ 105 ↔ 106 ↔ 107 ↔ 107 ↔ 110 ↔ 111
00900	112 ↔ 112 ↔ 113 ↔ 114 ↔ 115 ↔ 115 ↔ 116 ↔ 117
01000	117 ↔ 120 ↔ 121 ↔ 121 ↔ 122 ↔ 123 ↔ 124 ↔ 124
01100	125 ↔ 126 ↔ 127 ↔ 127 ↔ 130 ↔ 131 ↔ 131 ↔ 132
01200	133 ↔ 133 ↔ 134 ↔ 135 ↔ 135 ↔ 136 ↔ 136 ↔ 137
01300	140 ↔ 140 ↔ 141 ↔ 142 ↔ 142 ↔ 143 ↔ 143 ↔ 144
01400	145 ↔ 145 ↔ 146 ↔ 146 ↔ 147 ↔ 150 ↔ 150 ↔ 151
01500	151 ↔ 152 ↔ 153 ↔ 153 ↔ 154 ↔ 154 ↔ 155 ↔ 156
01600	156 ↔ 157 ↔ 160 ↔ 160 ↔ 161 ↔ 161 ↔ 162 ↔ 162
01700	163 ↔ 164 ↔ 164 ↔ 165 ↔ 165 ↔ 166 ↔ 166 ↔ 167
01800	167 ↔ 170 ↔ 170 ↔ 171 ↔ 171 ↔ 172 ↔ 172 ↔ 173
01900	173 ↔ 174 ↔ 174 ↔ 175 ↔ 176 ↔ 176 ↔ 177 ↔ 177
02000	
02100	200 ↔ 200 ↔ 200 ↔ 201 ↔ 201 ↔ 202 ↔ 203 ↔ 203
02200	204 ↔ 204 ↔ 205 ↔ 205 ↔ 206 ↔ 206 ↔ 207 ↔ 207
02300	210 ↔ 210 ↔ 211 ↔ 211 ↔ 212 ↔ 212 ↔ 213 ↔ 213
02400	214 ↔ 215 ↔ 215 ↔ 216 ↔ 216 ↔ 217 ↔ 217 ↔ 220
02500	221 ↔ 221 ↔ 222 ↔ 223 ↔ 223 ↔ 224 ↔ 224 ↔ 225
02600	226 ↔ 226 ↔ 227 ↔ 227 ↔ 230 ↔ 231 ↔ 231 ↔ 232
02700	232 ↔ 233 ↔ 234 ↔ 234 ↔ 235 ↔ 235 ↔ 236 ↔ 237
02800	237 ↔ 240 ↔ 241 ↔ 241 ↔ 242 ↔ 242 ↔ 243 ↔ 244
02900	244 ↔ 245 ↔ 246 ↔ 246 ↔ 247 ↔ 250 ↔ 250 ↔ 251
03000	252 ↔ 253 ↔ 253 ↔ 254 ↔ 255 ↔ 256 ↔ 256 ↔ 257
03100	260 ↔ 260 ↔ 261 ↔ 262 ↔ 262 ↔ 263 ↔ 264 ↔ 265
03200	265 ↔ 266 ↔ 267 ↔ 270 ↔ 270 ↔ 271 ↔ 272 ↔ 273
03300	273 ↔ 274 ↔ 275 ↔ 276 ↔ 277 ↔ 300 ↔ 301 ↔ 301
03400	302 ↔ 303 ↔ 304 ↔ 305 ↔ 306 ↔ 307 ↔ 310 ↔ 311
03500	312 ↔ 313 ↔ 314 ↔ 315 ↔ 317 ↔ 320 ↔ 322 ↔ 323
03600	325 ↔ 327 ↔ 331 ↔ 334 ↔ 340 ↔ 344 ↔ 351 ↔ 357
03700	
03800	   NMI ← START	;Reset??
03900	;Interrupt vectors.
04000	   LOC 177772
04100		NMI∧377
04200		(NMI⊗-10)∧377
04300		START∧377
04400		(START⊗-10)∧377
04500		TICK∧377
04600		(TICK⊗-10)∧377
04700	END